/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Purpose of this tool is to read N logfiles and to match these into one occupancy grid.
* First implementation will assume that the logfiles are already localized. 
*
* The first input file will be used as a reference. All logfiles are inserted into occupancy grids, 
* which are mathced together and then set to map.
*
*/
#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include <string>
#include "settings.hpp"
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "CParticleFilter.h"
#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG
#include "BetBdtLocalizer.h"
		
float topi(float da){
	if(da>M_PI)while(da>M_PI) da-=2.0*M_PI;
	else if(da<-M_PI)while(da<-M_PI) da+=2.0*M_PI;
	return da;
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/// Read Scan measurement file with arbitrary number of items (and allowing white space before '\n')
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
int forwardString(const char *buf, int begin,int items){
		int i=begin+1;
		int cnt = 0;
		while(cnt<items){
				if(buf[i]==' ') cnt++;
				i++;
		}
		return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas, Grid::pose &p ){
		float time_s;
		int i=0;
		char in_line[16096];
		char *ret;
		char c=0;
		
		ret = fgets (in_line, 16096, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return 0;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return 1;
				}
		}
		int cnt=0;
		while( (c!='\n')){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		meas.N = (cnt-3)/2;
		i =0;
		while(i<((cnt-3)/2)){
				sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
		}
	
		return 0;
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///DRAWING STUFF
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
		rgb[3*Nx*j+3*i]=r;
		rgb[3*Nx*j+3*i+1]=g;
		rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
		float *data = gr->getDataPtr();
		int Nx,Ny;
		gr->getDim(Nx,Ny);
		float v;
	
		for(int j=0;j<Ny;j++){
				for(int i=0;i<Nx;i++){
						v = data[Nx*j+i];
						//if(v>0.6) v = 0.99 ; else v = 0.1; 
				
						setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
				}
		}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											setRGBpixBold(RGB,x,y, Nx, r,g,b);
											i++;
											rr+=resolution;
									}
}

void drawLineThin(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											setRGBpix(RGB,x,y, Nx, r,g,b);
											i++;
											rr+=resolution;
									}
							}

void drawCross(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float angle = M_PI/4.0;
		for(int i=0;i<3;i++){
				drawLine(RGB, Nx,Ny,resolution, pix, piy,angle, length, r,g,b);
				angle+=M_PI/1.9;
		}
}


void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}

float randomi(){
	return (2.0 * (float)(rand()%10000) - 1.0)/10000.0;
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////

void saveScan(const char *fname, Grid::TScan &s){
		FILE *f = fopen(fname,"wt");
		if(f==NULL){
				fprintf(stderr,"Not good - unable to save!\n");
				exit(1);
		}

		for(int i=0;i<s.N;i++){
				fprintf(f,"%f %f\n",s.r[i],s.a[i]);
		}
		fclose(f);
}

void saveMeasurement(FILE *f, Grid::TScan &s, Grid::pose &p){
		if(f==NULL) return;
		fprintf(f,"%.4f %.4f %.4f ",p.x,p.y,p.a);
		for(int i=0;i<s.N-1;i++){
				fprintf(f,"%.3f %.3f ",s.r[i],s.a[i]);
		}
		fprintf(f,"%.3f %.3f\n",s.r[s.N-1],s.a[s.N-1]);
		fflush(f);
		
}

void printUsage(){
		fprintf(stderr,"\n-----------------------------------------------------------------\n");
		fprintf(stderr,"Matches a logged scan file to a given map\n");
		
		fprintf(stderr,"\nThe output is saved to 'out_result.jpg'\n");
		fprintf(stderr,"-----------------------------------------------------------------\n\n");
}



bool initializeLocalizers(gim::CSettings &settings, CBetBdtLocalizer **glocalizer, std::string map){
		float grid_size, resolution;
		float paramTh, paramfdist;
		
		settings.GetValue("BB-global.resolution", resolution);
		settings.GetValue("BB-global.distth", paramTh);
		settings.GetValue("BB-global.fdist", paramfdist);
		
		dPrint(1,"Initializing Global matcher:");
		dPrint(1,"map='%s' th=%.2f fdist=%.2f resolution=%.2f",map.c_str(),paramTh,paramfdist,resolution);
		
		*glocalizer = new CBetBdtLocalizer(map.c_str());
		
		(*glocalizer)->tuneParameters(paramfdist,paramTh);
		return true;
}

/**
* Sets a whole measurement file to the Occupancy Grid
* @param &settings Settings class
* @param *O Initialized Occupancy Grid 
* @param *meas_file The data log file
* @param *rgb Initialized image data ptr
* @param window_id The id of the window which the grid is drawn (Negative disables drawing)
**/
void setUpOccuMap(gim::CSettings &settings, TOccupancyGrid *O, const char *meas_file){
		FILE *log = fopen(meas_file,"rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		float max_laser_dist;
		settings.GetValue("BB-all.max_laser_distance", max_laser_dist);
		
		int cnt = 0;
		Grid::pose pos;
		std::vector< Grid::pose > poses;    ///All poses for previous map
		
		float enough_scan_poinsts;
		settings.GetValue("BB-all.enough_scan_points", enough_scan_poinsts);
		
		Grid::TScan meas;
		meas.alloc(enough_scan_poinsts);
		int Nx,Ny;
		O->getDim(Nx,Ny);
		while(!feof(log)){
				readScanLine(log,meas,pos);
				poses.push_back(pos);
				meas.filt(max_laser_dist);
				O->setScanFillProb(pos.x, pos.y ,pos.a,meas,0.3,0.9);
				cnt++;
		}
}


/**
* Match the whole data set to map
* Uses localized data to match the whole data set to the map
**/
void globalMatch(gim::CSettings &settings,
								 CBetBdtLocalizer *glocalizer,
								 TOccupancyGrid *O, const char *meas_file)
{
		int Nx,Ny;
		glocalizer->grid->getDim(Nx,Ny);
		
		CMatchStorage matches(50);
		CEnvironmentMeasurement2D meas(O,0.9); ///Make a measurement
		settings.GetValue("BB-global.initPoseX", meas.frame.x_ref);
		settings.GetValue("BB-global.initPoseY", meas.frame.y_ref);
		settings.GetValue("BB-global.initPoseA", meas.frame.a_ref);
		
		settings.GetValue("BB-global.searchX", meas.frame.x_err);
		settings.GetValue("BB-global.searchY", meas.frame.y_err);
		settings.GetValue("BB-global.searchA", meas.frame.a_err);
		
		dPrint(1,"Num_meas='%d' InitP (%.2f,%.2f,%.2f) Search: (%.2f,%.2f,%.2f)",meas.size(),
					 meas.frame.x_ref,meas.frame.y_ref,meas.frame.a_ref,
					 meas.frame.x_err,meas.frame.y_err,meas.frame.a_err);
		
		
		glocalizer->localize(meas,matches);
		fprintf(stderr," Done \n");
		matches.sort();
		fprintf(stderr,"\n###################################################\n");
		fprintf(stderr,"Pose estimate = (%f , %f , %f)\n",matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
		fprintf(stderr,"###################################################\n\n");
		
		Grid::pose reference(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
		
		///Set measurement set to the map
		FILE *log = fopen(meas_file,"rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		float max_laser_dist;
		settings.GetValue("BB-all.max_laser_distance", max_laser_dist);
		
		int cnt = 0;
		Grid::pose pos;
		std::vector< Grid::pose > poses;    ///All poses for previous map
		
		float enough_scan_poinsts;
		settings.GetValue("BB-all.enough_scan_points", enough_scan_poinsts);
		
		Grid::TScan smeas;
		Grid::pose pose_now=reference;
		Grid::pose prev,curr,diff;
		
		smeas.alloc(enough_scan_poinsts);
		
		readScanLine(log,smeas,prev);
		smeas.filt(max_laser_dist);
		pose_now = reference;
		glocalizer->setMeasurementToMap(smeas, pose_now.x,pose_now.y, pose_now.a, 0.9,false);
		
		while(!feof(log)){
				readScanLine(log,smeas,curr);
				smeas.filt(max_laser_dist);
				
				diff.setToDifferentialPose(curr,prev);
				pose_now = pose_now.integrateDifferential(diff);
				prev=curr;
				
				glocalizer->setMeasurementToMap(smeas, pose_now.x,pose_now.y, pose_now.a, 0.9,false);		
				cnt++;
		}
		dPrint(1,"Set %d scans to map... updating distance transform",cnt);
		glocalizer->updateDistTransform();
}


int main(int argc, char *argv[]){
	std::string configurationFile="logfile-conf.xml";
	int enough_scan_poinsts = 400; ///< enough to reserve memory for the scan points in the meas file.
		
		///Localizers
		CBetBdtLocalizer *glocalizer; 
		
		// Init Debug lib
		debugInit();
		debugSetGlobalDebugLvl(1);
	
		///Open & read settings file
		gim::CSettings settings;
		if(!settings.ReadSettingsXMLFile(configurationFile)){
				dPrint(1,"Failed to read the configuration file!!!");
				return 0;
		}
		int enableVisualization;
		settings.GetValue("BB-all.enableVisualization", enableVisualization);
		
		std::vector<std::string> logfiles;
		
		for(int i=1;i<argc;i++){ 
			logfiles.push_back(argv[i]);
			dPrint(1,"Logfile: '%s'",logfiles[i-1].c_str());
		}
		
		vector<TOccupancyGrid *> grids;
		
		///Initialize Occupancy grid 
		float grid_size;
		float occu_reso;
		settings.GetValue("Occu.grid_map_size", grid_size);
		settings.GetValue("Occu.resolution", occu_reso);
		
		
		///Set all logfiles to an occupancy grid
		for(int i=0;i<logfiles.size();i++){
			TOccupancyGrid *O = new TOccupancyGrid(grid_size,grid_size,occu_reso,Grid::TCoord2D(0,0));
			O->setGridToValue(0.5);
			setUpOccuMap(settings, O, logfiles[i].c_str());
			grids.push_back(O);
		}
		
		///a quick hack
		grids[0]->save("reference-map.occu");
		initializeLocalizers(settings, &glocalizer, "reference-map.occu");
		
		for(int i=1;i<grids.size();i++){
			globalMatch(settings,glocalizer,grids[i],logfiles[i].c_str());
		}
		
		glocalizer->save("result.occu");
		int Nx,Ny;
		glocalizer->grid->getDim(Nx,Ny);
		unsigned char *rgb = (unsigned char *) malloc(3*Nx*Ny);
		glocalizer->grid->fillRGB(rgb,1.0/255.0);
		saveJPEG("out_result.jpg", rgb,Nx, Ny);
	
	
	return 0;
}	